#include "SMCEngine.h"
#include "DataStorageStepper.h"

#include "log.h"

SMCEngine::SMCEngine(SMC& smc,DigitalOut& interruptPin):
    _smc(smc),
    _interruptPin(interruptPin)
{

}
SMCEngine::~SMCEngine(){}

void SMCEngine::begin()
{
    this->reset(_m1_mcc,0);
    this->reset(_m2_mcc,1);
    this->reset(_m3_mcc,2);
}

void SMCEngine::reset(stepper_compose_motion_t& mcc,uint8_t id)
{
    module_general_info_t& reg = DataStorageStepper::module_reg().module_general_reg.module_general_info[id];
    //mcc.cycle_motion_control.current_cycle_count = 0;
    mcc.cycle_motion_control.total_cycle_count   = reg.total_cycle_count_setup_reg.value;
    mcc.cycle_motion_control.state               = CycleMotionState::Inactive;

    mcc.linkage_motion_control.state                   = LinkageMotionState::Inactive;
    mcc.linkage_motion_control.current_linkage_steps   = 0;
    mcc.linkage_motion_control.total_linkage_steps     = reg.total_linkage_steps_stats_reg.value;
    mcc.linkage_motion_control.node                    = nullptr;
    mcc.linkage_motion_control.delay_100ms             = 0;
    
    mcc.id = id;
    switch(id)
    {
        case 0:mcc.manual = DataStorageStepper::module_reg().module_general_reg.motion_mode_reg.m1==0?true:false;break;
        case 1:mcc.manual = DataStorageStepper::module_reg().module_general_reg.motion_mode_reg.m2==0?true:false;break;
        case 2:mcc.manual = DataStorageStepper::module_reg().module_general_reg.motion_mode_reg.m3==0?true:false;break;
        default:break;
    }
    //清除中断
    _interruptPin = 0;

}

void SMCEngine::enable_control_all_drivers(bool m1_en,bool m2_en,bool m3_en)
{
    _smc.tb67s109a_electrical_control().device1.en = m1_en?1:0;
    _smc.tb67s109a_electrical_control().device2.en = m2_en?1:0;
    _smc.tb67s109a_electrical_control().device3.en = m3_en?1:0;
    _smc.update_electrical_devices();
}
void SMCEngine::enable_control_driver(MotorName name,bool en)
{
    switch (name)
    {
        case MotorName::M1:
        {
            _smc.tb67s109a_electrical_control().device1.en = en?1:0;
        }
        break;
        case MotorName::M2:
        {
            _smc.tb67s109a_electrical_control().device2.en = en?1:0;
        }
        break;
        case MotorName::M3:
        {
            _smc.tb67s109a_electrical_control().device3.en = en?1:0;
        }
        break;
        case MotorName::M4:break;
        default:break;
    } 
    _smc.update_electrical_devices();   
}



void SMCEngine::schedule()
{
    this->handle_cycle_motion(_m1_mcc);
    this->handle_cycle_motion(_m2_mcc);
    this->handle_cycle_motion(_m3_mcc);
}
void SMCEngine::m1_fire()
{
    _m1_mcc.cycle_motion_control.state = CycleMotionState::Start;
}
void SMCEngine::m2_fire()
{
    _m2_mcc.cycle_motion_control.state = CycleMotionState::Start;
}
void SMCEngine::m3_fire()
{
    _m3_mcc.cycle_motion_control.state = CycleMotionState::Start;
}
void SMCEngine::handle_cycle_motion(stepper_compose_motion_t& mcc)
{
    switch(mcc.cycle_motion_control.state)
    {
        case CycleMotionState::Inactive:break;
        case CycleMotionState::Start:
        {
            //循环模式不会清空
            mcc.cycle_motion_control.current_cycle_count = 0;
            mcc.cycle_motion_control.state = CycleMotionState::Restart;
            // //复位数据
            // this->reset(mcc,mcc.id);
            // //循环模式不会清空
            // mcc.cycle_motion_control.current_cycle_count = 0;
            // //清空当前运动步数
            // module_general_reg_t& reg = DataStorageStepper::module_reg().module_general_reg;
            // reg.module_general_info[mcc.id].current_motion_steps_stats_reg.value = 0;
            // // 关闭设备
            // _smc.tb67s109a_electrical_control().devices[mcc.id].en = 0;
            // _smc.update_electrical_devices();
            // //切换状态Ready
            // this->set_state(mcc.id,1);
            // mcc.cycle_motion_control.state = CycleMotionState::Ready;
            // log::debug("MCC(%d) Ready(%d)\r\n",mcc.id,mcc.cycle_motion_control.current_cycle_count);
        }
        break;
        case CycleMotionState::Restart:
        {   
            //复位
            this->reset(mcc,mcc.id);
            // 关闭设备
            _smc.tb67s109a_electrical_control().devices[mcc.id].en = 0;
            _smc.update_electrical_devices();
            //切换状态Ready
            this->set_state(mcc.id,1);
            mcc.cycle_motion_control.state = CycleMotionState::Ready;
        }
        break;
        case CycleMotionState::Ready:
        {
            //设置相对移动/绝对移动
            mcc.cycle_motion_control.state     = CycleMotionState::Running;
            mcc.linkage_motion_control.state   = LinkageMotionState::Start;
            //使能设备
            _smc.tb67s109a_electrical_control().devices[mcc.id].en = 1;
            _smc.update_electrical_devices();
            log::debug("MCC(%d) Running\r\n",mcc.id);
        }
        break;
        case CycleMotionState::Running:
        {
            //进入联动状态机
            if(this->handle_linkage_steps(mcc) == LinkageMotionState::Inactive){
               
               Serial.println("++Done++");
                Serial.println(mcc.cycle_motion_control.total_cycle_count);
                    Serial.println(mcc.cycle_motion_control.current_cycle_count);

                //判断循环次数是否完成
                if(mcc.manual ){
                    mcc.cycle_motion_control.state = CycleMotionState::Complete;//所有运动结束
                    log::debug("MCC(%d) Complete\r\n",mcc.id);
                    break;
                }
                
                 //循环次数+1
                mcc.cycle_motion_control.current_cycle_count+=1;
                if(mcc.cycle_motion_control.total_cycle_count <= mcc.cycle_motion_control.current_cycle_count){
                    Serial.println(mcc.cycle_motion_control.total_cycle_count);
                    Serial.println(mcc.cycle_motion_control.current_cycle_count);
                    mcc.cycle_motion_control.state = CycleMotionState::Complete;//所有运动结束
                }
                else{
                    mcc.cycle_motion_control.state = CycleMotionState::Restart;//完成了一次
                    log::debug("MCC(%d) Once Done:Start\r\n",mcc.id);
                }
            }
        }
        break;
        case CycleMotionState::Complete:
        {
            //切换状态Running
            this->set_state(mcc.id,2);
            mcc.cycle_motion_control.state = CycleMotionState::Inactive;
            log::debug("MCC(%d) Inactive\r\n",mcc.id);
            //设置完成中断
            DataStorageStepper::module_reg().module_general_reg.slave_interrupt_reg.m1 |= (1<<mcc.id);
            _interruptPin = 1;
        }
        break;
        default:break;
    }
}
void SMCEngine::set_state(uint8_t id,uint8_t value)
{
    switch (id)
    {
    case 0:DataStorageStepper::module_reg().module_general_reg.current_state_reg.m1.value = value; break;
    case 1:DataStorageStepper::module_reg().module_general_reg.current_state_reg.m2.value = value; break;
    case 2:DataStorageStepper::module_reg().module_general_reg.current_state_reg.m3.value = value; break;
    default:
        break;
    }   
}
bool SMCEngine::init_mcc(stepper_compose_motion_t& mcc)
{
    if(mcc.linkage_motion_control.current_linkage_steps==0){
        mcc.linkage_motion_control.node = linkage_steps_factory::linkage_steps_setups(mcc.id).peek_front();
    }else{
        mcc.linkage_motion_control.node = mcc.linkage_motion_control.node->next;
    }
    
    if(mcc.linkage_motion_control.node==nullptr){
        return false;
    }
    stepper_compose_data_t scd;
    scd.data[0]  = mcc.linkage_motion_control.node->memory.data[0];
    scd.data[1]  = mcc.linkage_motion_control.node->memory.data[1];
    mcc.linkage_motion_control.target_position_in_steps = scd.value;

    scd.data[0]  = mcc.linkage_motion_control.node->memory.data[2];
    scd.data[1]  = mcc.linkage_motion_control.node->memory.data[3];
    mcc.linkage_motion_control.delay_100ms         = scd.value;


    Serial.println(mcc.linkage_motion_control.target_position_in_steps);
    Serial.println(mcc.linkage_motion_control.delay_100ms);
    Serial.println(mcc.linkage_motion_control.total_linkage_steps);
    
    return true;
}
LinkageMotionState SMCEngine::handle_linkage_steps(stepper_compose_motion_t& mcc)
{
    switch(mcc.linkage_motion_control.state)
    {
        case LinkageMotionState::Inactive:break;
        case LinkageMotionState::Start:
        {   
            //初始化mcc参数
            this->init_mcc(mcc);
            //初始化步进
            _smc.mc(mcc.id).setCurrentPositionInSteps(0);
            _smc.mc(mcc.id).moveToPositionInSteps((int16_t)mcc.linkage_motion_control.target_position_in_steps);
            Serial.println(mcc.id);
            _smc.electronic_device(mcc.id).enable_clock();
            //切换状态Running
            this->set_state(mcc.id,1);
            mcc.linkage_motion_control.state        = LinkageMotionState::Running;
            log::debug("MCL(%d) Running(%d)\r\n",mcc.id,mcc.linkage_motion_control.current_linkage_steps);
        }
        break;
        case LinkageMotionState::Ready:
        {
            
        }
        break;
        case LinkageMotionState::Running:
        {
            //查询运动是否完成
            if(_smc.mc(mcc.id).motionComplete()){
                //停止步进时钟
                _smc.electronic_device(mcc.id).disable_clock();
                //判断点动/联动模式
                if(DataStorageStepper::module_reg().module_general_reg.motion_mode_reg.m1 ==0){
                    //切换状态完成
                    mcc.linkage_motion_control.state = LinkageMotionState::Complete;
                    log::debug("MCL Comp\r\n");
                }else{
                    mcc.linkage_motion_control.state = LinkageMotionState::Stop;//停止电机继续下一步
                    log::debug("MCL Stop\r\n");
                }
           }else{
                module_general_info_t& reg = DataStorageStepper::module_reg().module_general_reg.module_general_info[mcc.id];
                reg.current_motion_steps_stats_reg.value = _smc.mc(mcc.id).getCurrentPositionInSteps();
           }
        }
        break;
        case LinkageMotionState::Stop:
        {
            //禁止步进脉冲
            _smc.electronic_device(mcc.id).enable_clock();
            //设置状态停止
            this->set_state(mcc.id,0);
            //切换状态
            mcc.linkage_motion_control.last_millis = millis();
            mcc.linkage_motion_control.state = LinkageMotionState::WaitingDelay;
            log::debug("MCL WaitingDelay\r\n");
        }
        break;
        case LinkageMotionState::WaitingDelay:
        {   
            unsigned long td_millis = (millis()-mcc.linkage_motion_control.last_millis);
            if( td_millis < mcc.linkage_motion_control.delay_100ms*100){
                break;
            }
            //当前阶段联动步数+1
            mcc.linkage_motion_control.current_linkage_steps+=1;
            //联动组合动作
            if(mcc.linkage_motion_control.current_linkage_steps < mcc.linkage_motion_control.total_linkage_steps){
                mcc.linkage_motion_control.state = LinkageMotionState::StepOnceEnd;
                log::debug("MCL StepOnceEnd\r\n");
            }else{
                mcc.linkage_motion_control.state = LinkageMotionState::Complete;
                log::debug("MCL Complete\r\n");
            }
        }
        break;
        case LinkageMotionState::StepOnceEnd://单次运动结束
        {
            module_general_info_t& reg = DataStorageStepper::module_reg().module_general_reg.module_general_info[mcc.id];
            int32_t total_steps = (int32_t)reg.total_motion_steps_stats_reg.value+mcc.linkage_motion_control.target_position_in_steps;
            reg.total_motion_steps_stats_reg.value = total_steps;
            mcc.linkage_motion_control.state = LinkageMotionState::Start;
        }
        break;
        case LinkageMotionState::Complete:
        {
            //禁止时钟
            _smc.electronic_device(mcc.id).disable_clock();
           //计算累计总的运动步数
            module_general_info_t& reg = DataStorageStepper::module_reg().module_general_reg.module_general_info[mcc.id];
            int32_t total_steps = (int32_t)reg.total_motion_steps_stats_reg.value+mcc.linkage_motion_control.target_position_in_steps;
            reg.total_motion_steps_stats_reg.value = total_steps;

            //设置运动完成
            this->set_state(mcc.id,2);
            //切换为非活动状态
            mcc.linkage_motion_control.state = LinkageMotionState::Inactive;
            log::debug("MCL Finished\r\n");
        }
        break;
        default:break;
    }
    return mcc.linkage_motion_control.state;
}

void SMCEngine::update()
{

}
void SMCEngine::cleanup()
{

}

void SMCEngine::clear_interrupt()
{
    DataStorageStepper::module_reg().module_general_reg.slave_interrupt_reg.value = 0;
    _interruptPin = 0;
}